/*****************************************************************************
 * $Workfile:: main.c                                                        $
 * $Revision:: 00                                                            $
 * $JustDate::  2020/12/30                                                   $
 *
 *      GENIE V ECU
 *      serial communication interface
 *
 *****************************************************************************/

/***********************************************************************************************************************
Includes
***********************************************************************************************************************/

#include "common.h"
#include "serial.h"
#include "main.h"
#include "switches.h"
#include "display.h"
#include "eeprom.h"
#include "fet.h"
#include "control.h"
#include "nvm.h"
#include "alarm.h"
#include "speed_adjust.h"
#include "joystick.h"
#include "main.h"
#include "faults.h"
#include "battery.h"
#include "labview.h"
#include "load_sensing.h"
#include "calibrate_load_sensing.h"
#include "option_adjust.h"
#include "led.h"
#include "CAN.h"
#include "TransportCtrlLogic.h"
#include "FrequenceDecode.h"

#define PET_WATCHDOG    {R_WDT_Restart();}


struct TIMER TIMER_Data;
struct ST_HOUR_METER stHourMeter = {0, 0, 0}; ///<motor working time
struct MAIN MAIN_Data;
struct ST_ECU_LOCKLIB_CANDATA stECULockLibCANdata = {0};
union  UN_LIFT_LOCK_SET unLiftlocksets = {0};   ///<machine locks
struct ST_ECU_LOCKLIB stECULockLibStored = {0}; ///<stored data in EEPROM
union  UN_JOYSTICK unCanJoystickStatus = {0};    ///<can joystick status
union  UN_SWITCH  unCanSwitchStatus = {0};    ///<can switch status
union  UN_FAULT1 unCanFault1 = {0};          ///<can fault 1
union  UN_FAULT2 unCanFault2 = {0};          ///<can fault 2

UINT16  GTFtemp = 0;
UINT8   iFieldBrake = 0;
UINT8   iFieldBrake2 = 0;

unsigned int   FaultWord2 = 0;
unsigned int   FaultWord1 = 0;

UINT8 TrigAngleSwitch( unsigned char bx, unsigned int ui_Measured_Scissor_Angle  );
void AlignArray(unsigned int angle1[], unsigned int pressure1[]);


//#define CHASSIS_UP_PWM   lift_setup.bChassisLiftUpPwm
#define CHASSIS_UP_PWM   lift_setup.lift_up_curve.bPwmPosAdjust
#define LIFT_DRIVE_CUTOUT_OPTION  1
#define HEIGHT_OFFSET   70
#define HEIGHT_HYSTERESIS 20
//    #define LIFT_DRIVE_CUTOUT_OPTION  lift_options.fLevel_lift_drive_cutout     // enabled
static unsigned int sui_Stow_Counter = 0;                                  // STOW_TIME
//static UINT16  wCounter = 0;                                          // Counter for displaying test variables.
#define CONTROL_INTERVAL_MS(x)    ((x)/CONTROL_CYCLE)
#define STOW_TIME               CONTROL_INTERVAL_MS(500)
#define LOW_STOW_PRESSURE       160    //   No pressure 0x66  Min Pressure 0xCC=204

// The input bit for the Brake Release switch
#define BRAKE_RELEASE_SWITCH    u8GetStatus5B()      // If == 0, the lift is in brake release mode.

UINT8 fGlobal1ms=0;

unsigned char Pressure_Current_Voltage=0;
unsigned int   ui_Measured_Scissor_Angle_Main;
unsigned int   ui_Measured_Cylinder_Pressure;
unsigned int   ui_Measured_Cylinder_Pressure2;
int   n_Pressure_Percent_of_Maximum;
unsigned char u8J2_4Value=0;

/***********************************************************************************************************************
Pragma directive
***********************************************************************************************************************/
/* Start user code for pragma. Do not edit comment generated here */
/* End user code. Do not edit comment generated here */

/***********************************************************************************************************************
Global variables and functions
***********************************************************************************************************************/
/* Start user code for global. Do not edit comment generated here */
/* End user code. Do not edit comment generated here */
void R_MAIN_UserInit(void);

/***********************************************************************************************************************
* Function Name: main
* Description  : This function implements main function.
* Arguments    : None
* Return Value : None
***********************************************************************************************************************/
void vUserMain(void)
{
   static UINT8 u8_10msCnt=0;
   UINT8 u8DoubleChannelOutdoorCnt=0;
   UINT8 u8RecontCnt;
   unsigned int   ui_Load_Sensing_Angle_Sensor_Array[LOAD_SENSING_ARRAY_SIZE];
   unsigned int   ui_Load_Sensing_Pressure_Sensor_Array[LOAD_SENSING_ARRAY_SIZE];
   unsigned int   ui_NoLoad_Angle_Sensor_Array[LOAD_SENSING_ARRAY_SIZE];
   unsigned int   ui_NoLoad_Pressure_Sensor_Array[LOAD_SENSING_ARRAY_SIZE];
   unsigned int   ui_Up_Load_Sensing_Angle_Sensor_Array[LOAD_SENSING_ARRAY_SIZE];
   unsigned int   ui_Up_Load_Sensing_Pressure_Sensor_Array[LOAD_SENSING_ARRAY_SIZE];
   unsigned int   ui_Up_NoLoad_Angle_Sensor_Array[LOAD_SENSING_ARRAY_SIZE];
   unsigned int   ui_Up_NoLoad_Pressure_Sensor_Array[LOAD_SENSING_ARRAY_SIZE];
   unsigned int   ui_Dn_Load_Sensing_Angle_Sensor_Array[LOAD_SENSING_ARRAY_SIZE];
   unsigned int   ui_Dn_Load_Sensing_Pressure_Sensor_Array[LOAD_SENSING_ARRAY_SIZE];
   unsigned int   ui_Dn_NoLoad_Angle_Sensor_Array[LOAD_SENSING_ARRAY_SIZE];
   unsigned int   ui_Dn_NoLoad_Pressure_Sensor_Array[LOAD_SENSING_ARRAY_SIZE];
   unsigned int   ui_Max_Cylinder_Angle;
   unsigned char  uc_Sensor_Polarity;  // =1 if sensor values increase with scissor angle, =0 if they decrease
   unsigned char  uc_Active_Calibration_Status; // =1 if sensor values increase with scissor angle, =0 if they decrease
   unsigned char  uc_Sensor_Errors; // See Calibration routine for error definitions.
   unsigned int   ui_ErrorCounter;
   unsigned int   u16_InOutDoorSwitchFaultFitter=0;
   // unsigned long  ul_temp1, ul_temp2;
   // unsigned int    *ui_array_Load_Angle;   // ET
   // unsigned int    read_option_delay
   UINT32 dwPressureDifferenceDisableTimer;
   UINT8 optionCodeReadError;
   UINT8 uc_sw_left, uc_sw_right;

   INTERRUPTS_DISABLE;

   // initilize FET interface.
   // It must be first for hardware revision A as it share display and fet control lines.
   //
   FET_Init();
   vCanInitial();
   R_TAU0_Channel0_Start();
   R_TAU1_Channel0_Start();
   R_TAU1_Channel1_Start();

   INTERRUPTS_ENABLE;

   {R_WDT_Restart();}    /* restart watchdog timer */

   //
   // check for labview ( lift parameter update ) mode
   // and switch if the flag is set by boot code.
   //
   if(APP_MODE_LABVIEW ==  BootFlag)
   {
      BootFlag = 0;
      lbv_main();  // it is never return
   }

   BootFlag = 0;


   //
   // init rest of lift tasks
   //
   optionCodeReadError = NVM_LoadOptionCode();
   DISPLAY_Init();
   NVM_InitFault();
   NVM_InitLiftDrive();
   NVM_InitOutDoorMode();
   NVM_InitBatteryState();
   NVM_InitBatteryLevel();
   NVM_InitTripAngleState();
   NVM_InitSleepTimeState();
   NVM_InitHourMeter();
   switches();
   
   u8RecontCnt=0;
   while((optionCodeReadError !=0) && (u8RecontCnt < 5))
   {
	   R_IICA0_Create();
	   optionCodeReadError = NVM_LoadOptionCode();
	   u8RecontCnt++;
   }
   PET_WATCHDOG;
   switches();

   record_read(RECORD_LOAD_SENSING_PRESSURE_SENSOR_ARRAY
               , LOAD_SENSING_ARRAY_SIZE * 2
               , (UINT8 *)&ui_Load_Sensing_Pressure_Sensor_Array);
   record_read(RECORD_LOAD_SENSING_ANGLE_SENSOR_ARRAY
               , LOAD_SENSING_ARRAY_SIZE * 2
               , (UINT8 *)&ui_Load_Sensing_Angle_Sensor_Array);
   AlignArray(&ui_Load_Sensing_Angle_Sensor_Array[0], &ui_Load_Sensing_Pressure_Sensor_Array[0]);
   PET_WATCHDOG;
   switches();

   record_read(RECORD_NOLOAD_PRESSURE_SENSOR_ARRAY
               , LOAD_SENSING_ARRAY_SIZE * 2
               , (UINT8 *)&ui_NoLoad_Pressure_Sensor_Array);
   record_read(RECORD_NOLOAD_ANGLE_SENSOR_ARRAY
               , LOAD_SENSING_ARRAY_SIZE * 2
               , (UINT8 *)&ui_NoLoad_Angle_Sensor_Array);
   AlignArray(&ui_NoLoad_Angle_Sensor_Array[0], &ui_NoLoad_Pressure_Sensor_Array[0]);
   PET_WATCHDOG;
   switches();

   record_read(RECORD_UP_LOAD_SENSING_PRESSURE_SENSOR_ARRAY
               , LOAD_SENSING_ARRAY_SIZE * 2
               , (UINT8 *)&ui_Up_Load_Sensing_Pressure_Sensor_Array);
   record_read(RECORD_UP_LOAD_SENSING_ANGLE_SENSOR_ARRAY
               , LOAD_SENSING_ARRAY_SIZE * 2
               , (UINT8 *)&ui_Up_Load_Sensing_Angle_Sensor_Array);
   AlignArray(&ui_Up_Load_Sensing_Angle_Sensor_Array[0], &ui_Up_Load_Sensing_Pressure_Sensor_Array[0]);
   PET_WATCHDOG;
   switches();

   record_read(RECORD_UP_NOLOAD_PRESSURE_SENSOR_ARRAY
               , LOAD_SENSING_ARRAY_SIZE * 2
               , (UINT8 *)&ui_Up_NoLoad_Pressure_Sensor_Array);
   record_read(RECORD_UP_NOLOAD_ANGLE_SENSOR_ARRAY
               , LOAD_SENSING_ARRAY_SIZE * 2
               , (UINT8 *)&ui_Up_NoLoad_Angle_Sensor_Array);
   AlignArray(&ui_Up_NoLoad_Angle_Sensor_Array[0], &ui_Up_NoLoad_Pressure_Sensor_Array[0]);
   PET_WATCHDOG;
   switches();

   record_read(RECORD_DN_LOAD_SENSING_PRESSURE_SENSOR_ARRAY
               , LOAD_SENSING_ARRAY_SIZE * 2
               , (UINT8 *)&ui_Dn_Load_Sensing_Pressure_Sensor_Array);
   record_read(RECORD_DN_LOAD_SENSING_ANGLE_SENSOR_ARRAY
               , LOAD_SENSING_ARRAY_SIZE * 2
               , (UINT8 *)&ui_Dn_Load_Sensing_Angle_Sensor_Array);
   AlignArray(&ui_Dn_Load_Sensing_Angle_Sensor_Array[0], &ui_Dn_Load_Sensing_Pressure_Sensor_Array[0]);
   PET_WATCHDOG;
   switches();

   record_read(RECORD_DN_NOLOAD_PRESSURE_SENSOR_ARRAY
               , LOAD_SENSING_ARRAY_SIZE * 2
               , (UINT8 *)&ui_Dn_NoLoad_Pressure_Sensor_Array);
   record_read(RECORD_DN_NOLOAD_ANGLE_SENSOR_ARRAY
               , LOAD_SENSING_ARRAY_SIZE * 2
               , (UINT8 *)&ui_Dn_NoLoad_Angle_Sensor_Array);
   AlignArray(&ui_Dn_NoLoad_Angle_Sensor_Array[0], &ui_Dn_NoLoad_Pressure_Sensor_Array[0]);
   record_read(RECORD_LOAD_SENSING_CALIBRATION_STATUS
               , 1
               , &uc_Active_Calibration_Status);
   PET_WATCHDOG;
   switches();

   uc_Sensor_Polarity = max_angle(ui_Load_Sensing_Angle_Sensor_Array               // The array of stored scissor angles
                                  , ui_NoLoad_Angle_Sensor_Array                  // The array of stored scissor angles
                                  , LOAD_SENSING_ARRAY_SIZE                       // The number of elements in each stored array
                                  , (UINT16 *)&ui_Max_Cylinder_Angle);                     // The number of elements in each stored array

   MAIN_Data.wMax_Angle = ui_Max_Cylinder_Angle;
   MAIN_Data.fPressureValue = ui_Load_Sensing_Pressure_Sensor_Array[LOAD_SENSING_ARRAY_SIZE - 2] > 500;
   MAIN_Data.fAnglePolarity = (NVM_TripAngleState.anData[DOWN_LIMIT_ANGLE] <= NVM_TripAngleState.anData[UP_LIMIT_ANGLE]) ? 1 : 0;
   MAIN_Data.wMinimum_Angle = ui_NoLoad_Angle_Sensor_Array[0];
   MAIN_Data.dwSystemTime = 0;

   serial_init();
	
   // wCounter = 0;

   TIMER_Data.bSystemCycles = 0;
   EI();
	while(1)
	{
		if(fGlobal1ms)
		{
			fGlobal1ms=0;
			if(++u8_10msCnt>=10){u8_10msCnt=0;}
			vProcFrequencyDecode(&stFrequencyDecodeEgine,u8GetStatus5B());
			vCanTxFifoProcess();
			vPorcCtrlLogic();
			switch(u8_10msCnt)
			{
				case 1:
					vCanRxProcess();
			       	vCanTxProcess();
				break;
				default:break;
			}
		}
	}
   #if 0
   while(1)
   {
      while(TIMER_Data.bSystemCycles)
      {
      		u8J2_4Value = DUAL_OUTDOOR_SWITCH;
      	  if(!POTHOLE_LIMIT && MAIN_Data.fDescentLimit && TIMER_Data.wSystemTime > 5000)
		  {
		     MAIN_Data.bOutDoorMode = OUTDOOR_SWITCH;
			 NVM_SaveOutDoorMode(MAIN_Data.bOutDoorMode);
		  }
		  if(
			(
				(!MAIN_Data.bOutDoorMode && OUTDOOR_SWITCH) || 
				(MAIN_Data.bOutDoorMode && !OUTDOOR_SWITCH)||
				(MAIN_Data.bDoubleChannelOutDoorModeFault)
			)&&
			(TIMER_Data.wSystemTime > 5000)
		  )
		  {
		  	 if(u16_InOutDoorSwitchFaultFitter<100)
		  	 {
				u16_InOutDoorSwitchFaultFitter++;
			 }
			 else
			 {
				MAIN_Data.bOutDoorModeFault = 1;
			 }
		     
		  }
		  else
		  {
		  	 u16_InOutDoorSwitchFaultFitter = 0;
		     MAIN_Data.bOutDoorModeFault = 0;
		  }
      		  //MAIN_Data.bDoubleChannelOutDoorMode
		  if(
		  	((DUAL_OUTDOOR_SWITCH && OUTDOOR_SWITCH) || (!DUAL_OUTDOOR_SWITCH && !OUTDOOR_SWITCH))&&
		  	MAIN_Data.bDoubleChannelOutDoorMode
		  )
		  {
			if(u8DoubleChannelOutdoorCnt<5)
			{
				u8DoubleChannelOutdoorCnt++;
			}
			else
			{
				MAIN_Data.bDoubleChannelOutDoorModeFault = 1;
			}
		  }
		  else
		  {
			u8DoubleChannelOutdoorCnt=0;
			MAIN_Data.bDoubleChannelOutDoorModeFault = 0;
		  }
#ifdef DEBUG_TIME
         if(MAIN_Data.bMaximumCycles < TIMER_Data.bSystemCycles)
         {
            MAIN_Data.bMaximumCycles = TIMER_Data.bSystemCycles;
         }
#endif

         TIMER_Data.bSystemCycles = 0;
         MAIN_Data.dwSystemTime++;

         switches();
         if(((MAIN_Data.wTimeInState % 5) == 0x00) || (MAIN_Data.eState != MAIN_OPERATIONAL))
         {
            MAIN_Data.fDownLimit = TrigAngleSwitch( DOWN_LIMIT_ANGLE, ui_Measured_Scissor_Angle_Main );
            if(!CRASH_LIMIT)
            {
               if(MAIN_Data.bOutDoorMode && !lift_options.fKorea_option)
               {
                  MAIN_Data.fUpLimit = TrigAngleSwitch( OUTDOOR_LIMIT_ANGLE, ui_Measured_Scissor_Angle_Main );
               }
               else
               {
                  MAIN_Data.fUpLimit = TrigAngleSwitch( UP_LIMIT_ANGLE, ui_Measured_Scissor_Angle_Main );
               }
            }
            else
            {
               MAIN_Data.fUpLimit = 0;
            }
			//lift_options.fKorea_option&&CRASH_LIMIT
            MAIN_Data.fDescentLimit = TrigAngleSwitch( DESCENT_ANGLE_DIFF, ui_Measured_Scissor_Angle_Main );
            
            MAIN_Data.fPotholeTest = TrigAngleSwitch( POTHOLE_ANGLE, ui_Measured_Scissor_Angle_Main);
            if(lift_options.fOverload2_cutout)
            {  // 0807
               MAIN_Data.fOverloadLimit = TrigAngleSwitch( OVERLOAD_LIMIT, ui_Measured_Scissor_Angle_Main );
            }
            else
            {
               MAIN_Data.fOverloadLimit = MAIN_Data.fPotholeTest;
            }
         }
		 #if 0
		 if(MAIN_Data.eState == MAIN_OPERATIONAL)
		 {
		   vCanRxProcess();
	       vCanTxProcess();
		 }
		 #else 
		 vCanRxProcess();
	     vCanTxProcess();
		 #endif
         serial();

         switch(MAIN_Data.eState)
         {
            default:
               MAIN_Data.eState = MAIN_INIT;
               MAIN_Data.wTimeInState = 0;
            // no break statement here
            case MAIN_INIT:
               //
               // After stabilization of input swithes
               // it checks for chassis faults - shorted UP/DOWN switches.
               // NOTE1: The false error occur if one starts system
               //        with activated UP or DOWN switch.
               // NOTE2: The one of the chassis fauls
               //        put system in boot code to allow labview interface.
               //        The boot code intersept this fault first
               //        and keep ecu in boot mode allowing labview task to be
               //        invoked with `BootFlag.
               //
               MAIN_Data.wTimeInState++;
               if(MAIN_Data.wTimeInState > 5)
               {
                  MAIN_Data.wTimeInState = 0;
                  //
                  // check for test mode
                  //

                  if(CHASSIS_UP_SW)
                  {
                     MAIN_Data.fChassisFault = 1;
                  }
                  if(CHASSIS_DOWN_SW)
                  {
                     MAIN_Data.fChassisFault = 2;
                  }

                  MAIN_Data.eState = MAIN_WAIT_LINKUP;
               }
               vCheckDualOpenCircuit();
               break;
            case MAIN_WAIT_LINKUP:
               //
               // Wait till the communication is established to platform
               // controller. Check options and intit system parameters.
               // NOTE: the lift setup routine may. if neccesary, utlilize
               //       multiple cycles at this state.
               //
               if(xfer.fLinkActive)
               {
                  // the option byte received from platform controller
                  // initialize the system if no fault.
                  if(optionCodeReadError != 0)
                  {
                     NVM_SaveFault(FAULT_DIPSWITCH_ERROR);
                     MAIN_Data.eState = MAIN_LOCKOUT;
                     MAIN_Data.fOptionFault = 1;
                     FaultWord1 |= 0x0004;
					 unCanFault1.word |= 0x0004;
				  }
                  else
                  {
                     if(!Check_Option_Code(LIFT_OPTIONS_UINT16))
                     {
                        NVM_SaveFault(FAULT_DIPSWITCH_ERROR);
                        MAIN_Data.eState = MAIN_LOCKOUT;
                        *((UINT16 *)&lift_options) = 0;
                        MAIN_Data.fOptionFault = 1;
                     }
                     else
                     {
                        //
                        // request lift parameters from NVM to be copied in RAM
                        //
                        NVM_SetupInit();
                        MAIN_Data.eState = MAIN_WAIT_NVM_SETUP;
                     }
                  }
               }
               break;
            case MAIN_WAIT_NVM_SETUP:
               if(NVM_CheckSetupInit())
               {
                  MAIN_Data.eState = MAIN_WAIT_PLATFORM_INIT;
               }
               break;
            case MAIN_WAIT_PLATFORM_INIT:
                   MAIN_Data.wTimeInState++;

               //
               // check speed adjust mode
               //
                   if ( LIFT_DRIVE && SW_HORN)
                   {
                      MAIN_Data.eState = MAIN_SPEED_ADJUST;
                   }
				   else if(!KEY_SWITCH && SW_HORN && SW_HIGH)
				   {
						UINT32 u32_hex;
						MAIN_Data.eState = MAIN_GPS_UNLOCK_TEMPORARY_RANDOM_CODE;

						u32_hex = n_ECULockLibTempLockRandomMake();

						GPS_DATA.u8GPS_unlock_temporary_randomcode6 = u32_hex % 0x10;
						u32_hex = u32_hex / 0x10;
						GPS_DATA.u8GPS_unlock_temporary_randomcode5 = u32_hex % 0x10;
						u32_hex = u32_hex / 0x10;
						GPS_DATA.u8GPS_unlock_temporary_randomcode4 = u32_hex % 0x10;
						u32_hex = u32_hex / 0x10;
						GPS_DATA.u8GPS_unlock_temporary_randomcode3 = u32_hex % 0x10;
						u32_hex = u32_hex / 0x10;
						GPS_DATA.u8GPS_unlock_temporary_randomcode2 = u32_hex % 0x10;
						u32_hex = u32_hex / 0x10;
						GPS_DATA.u8GPS_unlock_temporary_randomcode1 = u32_hex % 0x10;
					

						GPS_DATA.u8GPS_unlock_temporary_checkcode1 = 0x4;
						GPS_DATA.u8GPS_unlock_temporary_checkcode2 = 0x3;
						GPS_DATA.u8GPS_unlock_temporary_checkcode3 = 0x2;
						GPS_DATA.u8GPS_unlock_temporary_checkcode4 = 0x1;
			
				   }
                   else if ( !KEY_SWITCH && SW_DRIVE && SW_HIGH
                             //&& lift_options.fOverload2_cutout
       //                   && LEVEL_SENSOR
                           )
                   {
                      MAIN_Data.eState = MAIN_TRIP_ANGLE_SAVE;
                   }
                   //
                   // Wait till the platform parameters are checked.
                   // The controll loop will switch to speed adjust
                   // or controll loop at this point.
                   //
                   else if ( JOYSTICK_Data.fPlatformReady )
                   {
                      if( (NVM_TripAngleState.anData[DOWN_LIMIT_ANGLE] == NVM_TripAngleState.anData[UP_LIMIT_ANGLE]) ||
                            ((MAIN_Data.fAnglePolarity && (NVM_TripAngleState.anData[OUTDOOR_LIMIT_ANGLE] >= NVM_TripAngleState.anData[UP_LIMIT_ANGLE])) ||
                             (!MAIN_Data.fAnglePolarity && (NVM_TripAngleState.anData[OUTDOOR_LIMIT_ANGLE] <= NVM_TripAngleState.anData[UP_LIMIT_ANGLE])))
                        )
                      {
                         MAIN_Data.eState = MAIN_TRIP_ANGLE_SAVE;
                      }
                      else
                      {
                         MAIN_Data.eState = MAIN_PROTA_INIT;
                      }
                   }
       //          if (ELEC3 == lift_setup.bLiftType)
       //             InitBrakeInt( );
                    //
                    // check brake release
                    //
               if(!KEY_SWITCH && CHASSIS_DOWN_SW && MAIN_Data.fChassisFault)
               {
                       if(MAIN_Data.wTimeInState > 50)
                       {
                  if((ELEC1 == lift_setup.bLiftType) /*|| (lift_setup.bLiftType == ELEC2)*/)
                  {
                     if ( MAIN_Data.fPotholeTest )
                     {
                        MAIN_Data.eState = MAIN_BRAKE_RELEASE;
                        MAIN_Data.wTimeInState = 0;
                     }
                  }
                  else if(ELEC3 == lift_setup.bLiftType)
                  {
                            if ( MAIN_Data.fPotholeTest )
                     {
                        if(!BRAKE_RELEASE_SWITCH)
                        {
                           MAIN_Data.eState = MAIN_BRAKE_RELEASE;
                           MAIN_Data.wTimeInState = 0;
                        }
                        else
                        {
                           NVM_SaveFault(FAULT_BRAKE_RELEASE_NEED_SWITCH_ON);
                        }
                     }
                     else
                     {
                        NVM_SaveFault(FAULT_BRAKE_RELEASE_NOT_SHOWED);
                     }
                  }
               }
               else
               {
                  MAIN_Data.eState = MAIN_WAIT_PLATFORM_INIT;
               }
            }
            else
            {
                  if(!BRAKE_RELEASE_SWITCH && (ELEC3 == lift_setup.bLiftType))
                  {
                     NVM_SaveFault(FAULT_BRAKE_RELEASE_SWITCH_ON);
                  }
                  else
                  {
                     if(MAIN_Data.fChassisFault == 1)
                     {   
						 NVM_SaveFault(FAULT_CHASSIS_UP_SWITCH);
					     FaultWord1 |= 0x0008;
						 // end Shawn@20160812
						 unCanFault1.word |= 0x0008;
					 }
                     if(MAIN_Data.fChassisFault == 2)
					 {
                        NVM_SaveFault(FAULT_CHASSIS_DOWN_SWITCH);
                        FaultWord1 |= 0x0008;
						// end Shawn@20160812
						unCanFault1.word |= 0x0008;
					 }
				  }
               }

               break;
            case MAIN_BRAKE_RELEASE:
               //
               // Warn brake release and release brakes
               //
               if(MAIN_Data.wTimeInState < 200)
               {
                  LIFT_Data.hd_fet = FET_HORN | FET_ALARM;
               }
               else if(MAIN_Data.wTimeInState == 200)
               {
                  if(ELEC3 == lift_setup.bLiftType)
                  {
                     LIFT_Data.hd_fet = FET_MCE | FET_ALARM | FET_BRAKES | FET_REVERSE;
                  }
               }
               else if(MAIN_Data.wTimeInState < 250)
               {
                  if(ELEC3 == lift_setup.bLiftType)
                  {
                     LIFT_Data.hd_fet |= FET_MCE | FET_ALARM;
                     if(CHASSIS_DOWN_SW)
                     {
                        LIFT_Data.hd_fet &= ~FET_FORWARD;
                        LIFT_Data.hd_fet |= FET_BRAKES | FET_REVERSE;
                     }
                     if(CHASSIS_UP_SW)
                     {
                        LIFT_Data.hd_fet &= ~FET_REVERSE;
                        LIFT_Data.hd_fet |= FET_BRAKES | FET_FORWARD;
                     }
                  }
                  else if(ELEC1 == lift_setup.bLiftType)   // Adding 1212 brake release
                  {
                     LIFT_Data.hd_fet = FET_ALARM | /*FET_MCE2  | */ FET_BRAKES /*| FET_FORWARD*/ ;  // Mode 1 & Mode 2
                  }
                  else
                  {
                     LIFT_Data.hd_fet = FET_ALARM | FET_BRAKES;
                  }
               }
               else if(MAIN_Data.wTimeInState < 350)
               {
                  LIFT_Data.hd_fet &=  ~FET_ALARM;
               }
               else
               {
                  MAIN_Data.wTimeInState = 200;
               }
               MAIN_Data.wTimeInState++;
               LIFT_setfets();
               break;
            case MAIN_PROTA_INIT:
               //
               // wait till Geine protocol finish initializaion
               //
               if(xfer.fPlatformInit)
               {
                  MAIN_Data.eState = MAIN_OPERATIONAL;
                  MAIN_Data.fInit = 1;
               }
               break;
            case MAIN_OPERATIONAL:
               //
               // All initialization is done. The lift control loop
               // is activated.
               //
               MAIN_Data.wTimeInState++;

               // Check if potholes are exteneded
               // if lift rized above down limit.
               //Korea machine has no pothole alarm //&& !lift_options.fKorea_option// Korea machine Add pothole alarm 250716
               if(!MAIN_Data.fPotholeTest && !POTHOLE_LIMIT )  
               {
                  if(!MAIN_Data.fPotholeGuardFailure)
                  {
                     NVM_SaveFault(FAULT_POTHOLE_GUARD_FAILURE);
                     MAIN_Data.fPotholeGuardFailure = 1;
					 FaultWord1 |= 0x0010;
					 unCanFault1.word |= 0x0010;
                  }
               }
               else
               {
                  MAIN_Data.fPotholeGuardFailure = 0;
				  FaultWord1 &= ~0x0010;
				  unCanFault1.word &= 0xFFEF;
               }

               //                    if (!MAIN_Data.fDownLimit && DOWN_LIMIT
               //                   && lift_options.fOverload2_cutout)
               //                    {
               //                        NVM_SaveFault( FAULT_MODE_SWITCH_FAILURE);
               //                    }

               /**********************************************************************************************/
               /******Test Code******Remove this section*************Test Code******Remove this section*******/
               //
                    // always check angle sensor failure
                    if (uc_Sensor_Errors & 0x0C)
                    {
                       MAIN_Data.fAngleFault = 1;
                       NVM_SaveFault( FAULT_ANGLE_INPUT);
					   FaultWord1 |= 0x0040;
					   // end Shawn@20160812
					   unCanFault1.word |= 0x0040;
                    }

               if(lift_options.fOverload2_cutout)
               {
                  if(MAIN_Data.fAnglePolarity)
                  {
                     if(!MAIN_Data.fUpDisable)
                     {
                        MAIN_Data.fUpDisable = (ui_Measured_Scissor_Angle_Main >= MAIN_Data.wMax_Angle);
                     }
                     else
                     {
                        MAIN_Data.fUpDisable = (ui_Measured_Scissor_Angle_Main >= (MAIN_Data.wMax_Angle - HEIGHT_HYSTERESIS));
                     }
                  }
                  else
                  {
                     if(!MAIN_Data.fUpDisable)
                     {
                        MAIN_Data.fUpDisable = (ui_Measured_Scissor_Angle_Main <= MAIN_Data.wMax_Angle);
                     }
                     else
                     {
                        MAIN_Data.fUpDisable = (ui_Measured_Scissor_Angle_Main <= (MAIN_Data.wMax_Angle + HEIGHT_HYSTERESIS));
                     }
                  }
                  if (ELEC3 == lift_setup.bLiftType  )              MAIN_Data.fUpDisable = 0;  // ET_GARY  09122011
                  if(LIFT_Data.calc_fet & FET_DOWN)
                  {
                     MAIN_Data.fUpDisable = 0;  // Re-enable the up motion only after the down switch has been activated.
                  }


                  if(uc_Sensor_Errors & 0x02)
                  {
                     ui_ErrorCounter++;
                     if(ui_ErrorCounter > 5)
                     {
                        ui_ErrorCounter = 5;
                        MAIN_Data.fPressureFault = 1;
                        NVM_SaveFault(FAULT_PRESSURE_INPUT);
                        FaultWord1 |= 0x0020;
						// end Shawn@20160812
						unCanFault1.word |= 0x0020;
					 }
                  }
                  else if(uc_Sensor_Errors & 0x01)
                  {
                           if (!MAIN_Data.fPotholeTest)
                     {
                        ui_ErrorCounter++;
                        if(ui_ErrorCounter > 150)
                        {
                           ui_ErrorCounter = 150;
                           MAIN_Data.fPressureFault = 1;
                           NVM_SaveFault(FAULT_PRESSURE_INPUT);
						   FaultWord1 |= 0x0020;
						   // end Shawn@20160812
						   unCanFault1.word |= 0x0020;
                        }
                     }
                  }

                  else if(uc_Sensor_Errors & 0x40)
                  {
                     ui_ErrorCounter++;
                     if(ui_ErrorCounter > 150)
                     {
                        ui_ErrorCounter = 150;
                        MAIN_Data.fPressureFault = 1;
                        NVM_SaveFault(FAULT_PRESSURE_INPUT2);
						FaultWord1 |= 0x0020;
						// end Shawn@20160812
						unCanFault1.word |= 0x0020;
                     }
                  }
                  else
                  {
                     ui_ErrorCounter = 0;
                  }
               }
               else
               {
                  MAIN_Data.fUpDisable = 0;
               }

               LIFT_Data.fLiftDriveDisable =
                  //                   (LIFT_DRIVE_CUTOUT_OPTION && !DOWN_LIMIT && !LEVEL_SENSOR )
                     (/*LIFT_DRIVE_CUTOUT_OPTION &&*/ (POTHOLE_LIMIT || !MAIN_Data.fDescentLimit) && !LEVEL_SENSOR )
                  || (!MAIN_Data.fPotholeTest && !POTHOLE_LIMIT ) //Korea machine has no pothole alarm//&& !lift_options.fKorea_option //Korea Add pothole
                  || (LIFT_Data.fOverload || LIFT_Data.fMotorFault
                      || MAIN_Data.fAngleFault || MAIN_Data.fPressureFault)
                  ;
               if(
                  (NVM_Fault.wLastFault == FAULT_BRAKE_COIL)
                  || (NVM_Fault.wLastFault == FAULT_LEFT_BRAKE_COIL)
                  || (NVM_Fault.wLastFault == FAULT_RIGHT_BRAKE_COIL)
               )
               {
                  LIFT_Data.fMotorFault = 1;
               }
			   vProcLiftUpPressLift();
               ALGOR_Lift();
               FET_Check();
               LIFT_setfets();
               vCheckOutDoorLimitSwitchFault();
			   
               // if lift is overloaded or battery below 9 volts
               // freeze all lift operations.

               if(/*LIFT_Data.fOverload
                ||*/ BATTERY_Data.fLowBatteryFault)
               {
                  MAIN_Data.eState = MAIN_LOCKOUT;
               }
               break;
            case MAIN_SPEED_ADJUST:
               // drive/lift curve tunning.
               // the task is never return.
               speed_adjust();
               break;
            case MAIN_GPS_UNLOCK_TEMPORARY_RANDOM_CODE:		
			   GPS_unlock_temporary_randomcode_display();
			   break;
            case MAIN_GPS_UNLOCK_TEMPORARY_CHECK_CODE:
				    GPS_unlock_temporary_checkcode_display();					
				    break;
            case MAIN_TRIP_ANGLE_SAVE:
               // Saves trip angle for down limit.
               LIFT_Data.calc_pwm1 = 0;
               LIFT_Data.calc_pwm2 = 0;
               if(!LEVEL_SENSOR)
               {
                  MAIN_Data.wTimeInState = 0;
                  LIFT_Data.hd_fet = 0; // set lift down
               }
               else if ( SW_ENABLE && (SW_LEFT || SW_RIGHT || SW_HORN || SW_LIFT) && !SW_HIGH && ( MAIN_Data.wTimeInState > 300 ) )
               {
                  if(MAIN_Data.wTimeInState > 301)
                  {
                     LIFT_Data.hd_fet = 0; // set lift down
                  }
                  else
                  {
                     if ( SW_ENABLE && SW_LEFT && !SW_RIGHT && !SW_HORN && !SW_LIFT && !SW_HIGH)
                     {
                        NVM_SaveTripAngleState(UP_LIMIT_ANGLE, ui_Measured_Scissor_Angle_Main);
                     }
                     else if ( SW_ENABLE && !SW_LEFT && SW_RIGHT && !SW_HORN && !SW_LIFT && !SW_HIGH)
                     {
                        NVM_SaveTripAngleState(DOWN_LIMIT_ANGLE, ui_Measured_Scissor_Angle_Main);
                     }
                     else if ( SW_ENABLE && !SW_LEFT && !SW_RIGHT && SW_HORN && !SW_LIFT && !SW_HIGH)
                     {
                        if(MAIN_Data.fAnglePolarity)
                        {
                           NVM_SaveTripAngleState(OVERLOAD_LIMIT, ui_Measured_Scissor_Angle_Main + SET_HEIGHT_OFFSET_ANGLE);
                        }
                        else
                        {
                           NVM_SaveTripAngleState(OVERLOAD_LIMIT, ui_Measured_Scissor_Angle_Main - SET_HEIGHT_OFFSET_ANGLE);
                        }
                     }
                     else if ( SW_ENABLE && !SW_LEFT && !SW_RIGHT && !SW_HORN && SW_LIFT && !SW_HIGH)
                     {
                        NVM_SaveTripAngleState(OUTDOOR_LIMIT_ANGLE, ui_Measured_Scissor_Angle_Main);
                     }
                     MAIN_Data.wTimeInState++;
                  }
               }
               else if ( SW_ENABLE && SW_LEFT && !SW_RIGHT && !SW_HORN && !SW_LIFT && !SW_HIGH)
               {
                  MAIN_Data.wTimeInState++;
                  LIFT_Data.hd_fet = FET_HORN | FET_ALARM;
               }
               else if ( SW_ENABLE && !SW_LEFT && SW_RIGHT && !SW_HORN && !SW_LIFT && !SW_HIGH)
               {
                  MAIN_Data.wTimeInState++;
                  LIFT_Data.hd_fet = FET_HORN | FET_ALARM;
               }
               else if ( SW_ENABLE && !SW_LEFT && !SW_RIGHT && SW_HORN && !SW_LIFT && !SW_HIGH)
               {
                  MAIN_Data.wTimeInState++;
                  LIFT_Data.hd_fet = FET_HORN | FET_ALARM;
               }
               else if ( SW_ENABLE && !SW_LEFT && !SW_RIGHT && !SW_HORN && SW_LIFT && !SW_HIGH)
               {
                  MAIN_Data.wTimeInState++;
                  LIFT_Data.hd_fet = FET_HORN | FET_ALARM;
               }
               else if ( SW_ENABLE && !SW_HORN && !SW_LIFT && SW_HIGH)
               {
                  // press left switch
                  if(SW_LEFT && !uc_sw_left)
                  {
                     if(NVM_TripAngleState.anData[DESCENT_ANGLE_DIFF] > 0)
                     {
                        NVM_TripAngleState.anData[DESCENT_ANGLE_DIFF] -= 1;
                        NVM_SaveTripAngleState(DESCENT_ANGLE_DIFF, NVM_TripAngleState.anData[DESCENT_ANGLE_DIFF]);
                      }
                  }
                  // press right switch
                  else if(SW_RIGHT && !uc_sw_right)
                  {
                     if(NVM_TripAngleState.anData[DESCENT_ANGLE_DIFF] < 99)
                     {
                        NVM_TripAngleState.anData[DESCENT_ANGLE_DIFF] += 1;
                        NVM_SaveTripAngleState(DESCENT_ANGLE_DIFF, NVM_TripAngleState.anData[DESCENT_ANGLE_DIFF]);
                     }
                  }
                  // save old switch
                  uc_sw_left = SW_LEFT;
                  uc_sw_right = SW_RIGHT;
               }
               /*
                * Chassis Key Switch points to chassis
                */
               else if(!KEY_SWITCH && !MAIN_Data.fChassisFault && (CHASSIS_UP_SW || CHASSIS_DOWN_SW))
               {
                  /*
                   * Lifting up is prohibited if pothole stuck retracted
                  * and lift is rised above lower limit.
                  * or lift is above upper limit.
                  */
                  MAIN_Data.wTimeInState = 0;
                  if((CHASSIS_UP_SW && !CHASSIS_DOWN_SW)
                     && LEVEL_SENSOR
                     //             && (MAIN_Data.fUpLimit //UP_LIMIT
                     //                      && !MAIN_Data.fUpDisable
                    )
                  {
                     // LIFT_UP_ON <ET>
                     LIFT_Data.calc_pwm1 = CHASSIS_UP_PWM;
                     LIFT_Data.hd_fet = (FET_UP | FET_MCE);  //GTF removed FET_MCE 5-9-09
                     if(ELEC3 == lift_setup.bLiftType)
                        LIFT_Data.hd_fet |= FET_MCE2;
                     //                         fEnable = 1;
                  }
                  else if(!CHASSIS_UP_SW && CHASSIS_DOWN_SW)
                  {
                     LIFT_Data.hd_fet = FET_DOWN ; // set lift down
                     //                      fEnable = 1;
                  }
               }
               else
               {
                  MAIN_Data.wTimeInState = 0;
                  LIFT_Data.hd_fet = 0; // set lift down
                  //                MAIN_Data.eState = MAIN_WAIT_PLATFORM_INIT;
               }
               LIFT_setfets();
               break;
            case MAIN_LOCKOUT:
               //
               // All lift operation is froze till power is cycled.
               //
               if(1 == MAIN_Data.fOptionFault
                  && LIFT_DRIVE && SW_HIGH)
               {
                  MAIN_Data.eState = MAIN_SPEED_ADJUST;
               }

               LIFT_Data.hd_fet = 0;
               LIFT_Data.calc_pwm1 = 0;
               LIFT_Data.calc_pwm2 = 0;
               LIFT_setfets();
               break;
         }
         BATTERY_Monitor();

         calibrate_load_sense(&ui_Load_Sensing_Angle_Sensor_Array[0]       // Output: The array of stored scissor angles
                              , &ui_Load_Sensing_Pressure_Sensor_Array[0]    // Output: The cooresponding array of stored cylinder pressures
                              , &ui_NoLoad_Angle_Sensor_Array[0]          // Output: The array of stored scissor angles
                              , &ui_NoLoad_Pressure_Sensor_Array[0]       // Output: The cooresponding array of stored cylinder pressures
                              , &ui_Up_Load_Sensing_Angle_Sensor_Array[0]       // Output: The array of stored scissor angles
                              , &ui_Up_Load_Sensing_Pressure_Sensor_Array[0]    // Output: The cooresponding array of stored cylinder pressures
                              , &ui_Up_NoLoad_Angle_Sensor_Array[0]          // Output: The array of stored scissor angles
                              , &ui_Up_NoLoad_Pressure_Sensor_Array[0]       // Output: The cooresponding array of stored cylinder pressures
                              , &ui_Dn_Load_Sensing_Angle_Sensor_Array[0]       // Output: The array of stored scissor angles
                              , &ui_Dn_Load_Sensing_Pressure_Sensor_Array[0]    // Output: The cooresponding array of stored cylinder pressures
                              , &ui_Dn_NoLoad_Angle_Sensor_Array[0]          // Output: The array of stored scissor angles
                              , &ui_Dn_NoLoad_Pressure_Sensor_Array[0]       // Output: The cooresponding array of stored cylinder pressures
                              , LOAD_SENSING_ARRAY_SIZE                // Input: The number of elements in each stored array
                              , &uc_Active_Calibration_Status             // Output: The calibration status and errors
                              //                      ,&ui_Max_Cylinder_Angle
                             );

         n_Pressure_Percent_of_Maximum =
            load_sense(&ui_Load_Sensing_Angle_Sensor_Array[0]                // Input: The array of stored scissor angles
                       , &ui_Load_Sensing_Pressure_Sensor_Array[0]          // Input: The cooresponding array of stored cylinder pressures
                       , &ui_NoLoad_Angle_Sensor_Array[0]                // Input: The array of stored scissor angles
                       , &ui_NoLoad_Pressure_Sensor_Array[0]             // Input: The cooresponding array of stored cylinder pressures
                       , &ui_Up_Load_Sensing_Angle_Sensor_Array[0]             // Input: The array of stored scissor angles
                       , &ui_Up_Load_Sensing_Pressure_Sensor_Array[0]          // Input: The cooresponding array of stored cylinder pressures
                       , &ui_Up_NoLoad_Angle_Sensor_Array[0]                // Input: The array of stored scissor angles
                       , &ui_Up_NoLoad_Pressure_Sensor_Array[0]             // Input: The cooresponding array of stored cylinder pressures
                       , &ui_Dn_Load_Sensing_Angle_Sensor_Array[0]             // Input: The array of stored scissor angles
                       , &ui_Dn_Load_Sensing_Pressure_Sensor_Array[0]          // Input: The cooresponding array of stored cylinder pressures
                       , &ui_Dn_NoLoad_Angle_Sensor_Array[0]                // Input: The array of stored scissor angles
                       , &ui_Dn_NoLoad_Pressure_Sensor_Array[0]             // Input: The cooresponding array of stored cylinder pressures
                       , LOAD_SENSING_ARRAY_SIZE                      // Input: The number of elements in each stored array
                       , &ui_Measured_Scissor_Angle_Main                 // Output: The measured scissor angle as measure in this function
                       , &ui_Measured_Cylinder_Pressure               // Output: The measured cylinder pressure as measured in this function
                       , &ui_Measured_Cylinder_Pressure2
                      );

         MAIN_Data.wMeasured_Scissor_Angle = ui_Measured_Scissor_Angle_Main;

         if(Fets.word & (FET_FORWARD | FET_RIGHT | FET_LEFT | FET_REVERSE))
            dwPressureDifferenceDisableTimer = MAIN_Data.dwSystemTime;
         if(((MAIN_Data.dwSystemTime - dwPressureDifferenceDisableTimer) < 200l)   && (ui_Measured_Cylinder_Pressure > (MAXIMUM_ALLOWABLE_PRESSURE_SENSOR_VOLTAGE - 1)))          //
            uc_Sensor_Errors = check_for_sensor_errors(ui_Measured_Scissor_Angle_Main   //
                               , (MAXIMUM_ALLOWABLE_PRESSURE_SENSOR_VOLTAGE - 1)); //
         else
            uc_Sensor_Errors = check_for_sensor_errors(ui_Measured_Scissor_Angle_Main   //
                               , ui_Measured_Cylinder_Pressure);   //
         if(Pressure_Current_Voltage == 1)
         {
            uc_Sensor_Errors |= check_for_sensor_errors2(ui_Measured_Cylinder_Pressure    //
                                , ui_Measured_Cylinder_Pressure2);  //  Check the fault for the sum of pressures
         }
         if((MAIN_Data.fAnglePolarity && (MAIN_Data.wMeasured_Scissor_Angle > (MAIN_Data.wMax_Angle - (HEIGHT_OFFSET - 13))))
            || (!MAIN_Data.fAnglePolarity && (MAIN_Data.wMeasured_Scissor_Angle < (MAIN_Data.wMax_Angle + (HEIGHT_OFFSET - 13)))))
         {
            MAIN_Data.fNearMaxHeight = 1;
            if(MAIN_Data.fAnglePolarity)
               MAIN_Data.wHeightOffset = MAIN_Data.wMeasured_Scissor_Angle + HEIGHT_OFFSET - 13 - MAIN_Data.wMax_Angle;
            else
               MAIN_Data.wHeightOffset = MAIN_Data.wMax_Angle + HEIGHT_OFFSET - 13 - MAIN_Data.wMeasured_Scissor_Angle;
         }
         else
         {
            MAIN_Data.fNearMaxHeight = 0;
            MAIN_Data.wHeightOffset = 0;
         }

         // ET_WORKING <CLEAR_OL>
         // If we OL and went down, remove OL when no pressure detected
         //
         if ( MAIN_Data.fPotholeTest && (ui_Measured_Cylinder_Pressure < ui_NoLoad_Pressure_Sensor_Array[0]))
         {
            sui_Stow_Counter++;
            if(sui_Stow_Counter > STOW_TIME)
            {
               LIFT_Data.fOverload = FALSE;     // The weight has been removed.  Allow Movement again. We are at Stow.
               LIFT_Data.fLoad99 = FALSE;
               LIFT_Data.fLoad90 = FALSE;
               LIFT_Data.fLoad80 = FALSE;
               sui_Stow_Counter = STOW_TIME;
               LIFT_Data.nOverloadPercent = 0;
            }
         }
         else
         {
            sui_Stow_Counter = 0;
         }
         // ET_WORKING <CLEAR_OL>
         ALGOR_Overload(n_Pressure_Percent_of_Maximum);
         DISPLAY_Task();
		 ECULockControlProcess();
         if(ELEC1 == lift_setup.bLiftType || ELEC3 == lift_setup.bLiftType /*|| lift_setup.bLiftType == ELEC2*/)
            MotorFaultMonitor();
         ALARM_Process();
		 ALARM_Process2();
         SLEEP_Process();
         if(lift_options.fOverload2_cutout)
            LED_Process();
         NVM_Task();
         PET_WATCHDOG;
      }

      //        //
      //        // check stack overflow
      //        //
      //        for ( IX = 0; IX < GUARD_SIZE; IX++)
      //        {
      //            if ( abGuard[ IX] )
      //            {
      //                //ERROR_Abort( );
      //            }
      //        }
   }
   #endif
}

/*******************************************************************************
 *
 * PROCEDURE:  INT_timerV()
 *
 * DESCRIPTION: ISR routine of timer V.
 *              It maintain 1 millisecond boot timer TIMER_Data.wSystemTimer
 *              that is good for 59.999 seconds after reset
 *              and lift control task  semaphor TIMER_Data.bSystemCycles
 *              initiated every 10 millisecond.
 *              The lost cycles aren't detected.
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 * TIMER_Data       R/W   owner
 ******************************************************************************/

// 200us  timer
#if (SYSTEM_CYCLE <= 0)
#undef SYSTEM_CYCLE
#define SYSTEM_CYCLE 1
#endif
extern void
INT_timerV(void)
{
   static UINT16 u16_ms;
   static UINT8 u8_1ms_cnt=0;
	vCanFastRxProcess();
	if(++u8_1ms_cnt>=5){u8_1ms_cnt=0;}
   switch(u8_1ms_cnt)
   {
	 case 1:
	 	   if(MAIN_Data.eState!=MAIN_INIT)
	 	   {
		   	 FET_Update();
	 	   }
		   TIMER_Data.wTestClock++;
		   if(TIMER_Data.wSystemTime < 59999)
		   {  // timer since power up
		      TIMER_Data.wSystemTime++;
		   }

		   if(TIMER_Data.bTenMillisecondTimer)
		   {
		      TIMER_Data.bTenMillisecondTimer--;
		   }
		   else
		   {
		      TIMER_Data.bTenMillisecondTimer = SYSTEM_CYCLE - 1;
		      TIMER_Data.bSystemCycles++;
		      TIMER_Data.dwSystemClock++;
		   }
	 break;
	 case 2:
	   if (Fets.word & (FET_UP | FET_FORWARD | FET_REVERSE | FET_RIGHT | FET_LEFT))
	   {
	       u16_ms++;
	       if (u16_ms >= 3600)
	       {
	           u16_ms = 0;
	           stHourMeter.u32Fraction++;
	       }
	   }
	   else
	   {
	      if (stHourMeter.u32Fraction > stHourMeter.u32FractionOld)
	      {
	         NVM_SaveHourMeter();
	         stHourMeter.u32FractionOld = stHourMeter.u32Fraction;
	      }
	   }
	 break;
	 case 3:
	 	
	 break;
	 case 4:
	 	fGlobal1ms=1;
	 	PET_WATCHDOG;
	 break;
	 default:break;
   }  
}

/*******************************************************************************
 *
 * PROCEDURE:  TrigAngleSwitch()
 *
 * DESCRIPTION: It determines measured angle is above or below trip angle.
 *              Return 1 means below, return 0 means above.
 *              It is invoked once per system cycle 10 milliseonds
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 * SWITCH_Data        R/W owner
 ******************************************************************************/

UINT8
TrigAngleSwitch( unsigned char bx, unsigned int ui_Measured_Scissor_Angle  )
{
   static UINT8 uc_Angle_Filter[ NVM_ANGLE_RECORD_SIZE];
   //    if (lift_options.fOverload2_cutout /*&& bx != SPEED_MODE_ANGLE*/)
   // {
   uc_Angle_Filter[ bx] = uc_Angle_Filter[ bx] << 1;
   if (MAIN_Data.fAnglePolarity)
   {
//       return (ui_Measured_Scissor_Angle < NVM_GetTripAngleState( ));
      if(ui_Measured_Scissor_Angle < NVM_GetTripAngleState(bx))
         uc_Angle_Filter[ bx] |= 0x01;
      else
         uc_Angle_Filter[ bx] &= 0xFE;
   }
   else
   {
//       return (ui_Measured_Scissor_Angle > NVM_GetTripAngleState( ));
      if(ui_Measured_Scissor_Angle > NVM_GetTripAngleState(bx))
         uc_Angle_Filter[ bx] |= 0x01;
      else
         uc_Angle_Filter[ bx] &= 0xFE;
   }
   if((uc_Angle_Filter[ bx] & 0x3F) == 0x3F)
      uc_Angle_Filter[ bx] = 0xFF;
   else if((uc_Angle_Filter[ bx] & 0x3F) == 0x00)
      uc_Angle_Filter[ bx] = 0x00;
   else if((uc_Angle_Filter[ bx] & 0x80) == 0x80)
      uc_Angle_Filter[ bx] |= 0xC0;
   else
      uc_Angle_Filter[ bx] &= 0x3F;

   return ((uc_Angle_Filter[ bx] & 0x80) == 0x80);
   // }
   // else if (lift_options.fDescent_delay)
   //    return (!POTHOLE_LIMIT);
   // else
   //    return (DOWN_LIMIT);
}

/*******************************************************************************
 *
 * PROCEDURE:  AlignArray()
 *
 * DESCRIPTION: Aligns pressure and angle sensor arrays
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 * SWITCH_Data        R/W owner
 ******************************************************************************/

void
AlignArray(unsigned int angle1[], unsigned int pressure1[])
{
   UINT8 IX;
   unsigned int   *angle;
   unsigned int   *pressure;

   angle = &angle1[0];
   pressure = &pressure1[0];
   for(IX = 1; IX < LOAD_SENSING_ARRAY_SIZE; IX++)
   {
      *(angle + IX) = *(angle + IX) & 0x07FF;
      *(pressure + IX) = *(pressure + IX) & 0x03FF;
      if(*(angle + IX) == *(angle + IX - 1))
      {
         for(; IX < LOAD_SENSING_ARRAY_SIZE ; IX++)
         {
            *(angle + IX) = *(angle + IX - 1);
         }
      }
   }
}

/* End user code. Do not edit comment generated here */

























































































































































































































































































































































































































































